/** 
 * @main    Plik z definicja klasy Identyfikacja
 * @file    identyfikacja.cpp
 * @author  Grzegorz Sejnota
 * @created on 19 marca 2009, 10:41
 * @version 0.0001
 */

#include "identyfikacja.h"
#include "wyjatki.h"
#include <iostream>
using namespace std;

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++ POCZATEK [definicji klasy Identyfikacja] ++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

//======= konstruktor ==========================================================
/**
 * Konstruktor klasy Identyfikacja.
 */
Identyfikacja::Identyfikacja(){
    this->dA = 1;
    this->dB = 1;
    this->dC = 1;
    this->k = 1;
    this->alfa = 1;
    numerProbki = 0;
    pamiecU = deque<double>(dB+1+k, 0.0);
    pamiecY = deque<double>(dA+1, 0.0);
    pamiecE = deque<double>(dC, 0.0);
    wektorTheta = Macierz(dB+1+dA+dC, 1, 0.0);
    wektorFi = Macierz(dB+1+dA+dC, 1, 0.0);
    macierzP = Macierz::I(dB+1+dA+dC)*100;
    /*
    cout<<endl<<endl<<"====Ident===="<<endl;
    cout<<"wektorTheta: "<<endl<<wektorTheta<<endl;
    cout<<"wektorFi: "<<endl<<wektorFi<<endl;
    cout<<"macierzP: "<<endl<<macierzP<<endl;
     **/
}

//======= konstruktor ==========================================================
/**
 * Konstruktor klasy Identyfikacja.
 *      @param m - vector vectorow wartosciami macierzy
 */
Identyfikacja::Identyfikacja(int dA, int dB, int dC, int k, double alfa){
    this->dA = dA;
    this->dB = dB;
    this->dC = dC;
    this->k = k;
    this->alfa = alfa;
    numerProbki = 0;
    pamiecU = deque<double>(dB+1+k, 0.0);
    pamiecY = deque<double>(dA+1, 0.0);
    pamiecE = deque<double>(dC, 0.0);
    wektorTheta = Macierz(dB+1+dA+dC, 1, 0.1);
    wektorFi = Macierz(dB+1+dA+dC, 1, 0.0);
    macierzP = Macierz::I(dB+1+dA+dC)*100;
    /*
    cout<<endl<<endl<<"====Ident===="<<endl;
    cout<<"wektorTheta: "<<endl<<wektorTheta<<endl;
    cout<<"wektorFi: "<<endl<<wektorFi<<endl;
    cout<<"macierzP: "<<endl<<macierzP<<endl;
     **/
}

//======= konstruktor kopiujacy ================================================
/**
 * Konstruktor kopiujacy klasy Identyfikacja
 *      @param &wzor - referencja do obiektu klasy Identyfikacja, 
 *                                        ktory ma byc skopiowany.
 */
Identyfikacja::Identyfikacja(const Identyfikacja & wzor) {
    this->dA = wzor.dA;
    this->dB = wzor.dB;
    this->dC = wzor.dC;
    this->k = wzor.k;
    this->alfa = wzor.alfa;
    numerProbki = 0;
    pamiecU = wzor.pamiecU;
    pamiecY = wzor.pamiecY;
    pamiecE = wzor.pamiecE;
    wektorTheta = wzor.wektorTheta;
    wektorFi = wzor.wektorFi;
    macierzP = wzor.macierzP;
}

//======= przeladowany operator przypisania ====================================
/** 
 * Przeladowany operator przypisania =. 
 *
 *     @param wzor 
 *     @return referencja 
 */
Identyfikacja & Identyfikacja::operator=(const Identyfikacja &wzor) {
    this->dA = wzor.dA;
    this->dB = wzor.dB;
    this->dC = wzor.dC;
    this->k = wzor.k;
    this->alfa = wzor.alfa;
    numerProbki = 0;
    pamiecU = wzor.pamiecU;
    pamiecY = wzor.pamiecY;
    pamiecE = wzor.pamiecE;
    wektorTheta = wzor.wektorTheta;
    wektorFi = wzor.wektorFi;
    macierzP = wzor.macierzP;
    return *this;
}

//======= aktualizujFi =========================================================
/**
 * Funkcja aktualizujaca wektor wierszowy pomiarow Fi na podstawie wartosci 
 * pamieci wejsc i wyjsc.
 */
void Identyfikacja::aktualizujFi() {
    //cout << "aktualizuj Fi" << endl;
    for (int i = 0; i <= dB; i++)
        wektorFi.at(i, 0) = pamiecU.at(i + k);
    for (int i = 1; i <= dA; i++)
        wektorFi.at(dB + i, 0) = -pamiecY.at(i);
    for (int i = 0; i < dC; i++)
        wektorFi.at(dB + 1 + dA + i, 0) = pamiecE.at(i);
    //cout<< "\n wektorFi: " << endl <<wektorFi <<endl;
}

//======= aktualizujPamiec =====================================================
/**
 * Funkcja aktualizujaca wektory pamieci wejsc i wyjsc.
 *      @param u - wartosc obecnego wejscia
 *      @param y - wartosc obecnego wyjscia
 */
void Identyfikacja::aktualizujPamiec(double u, double y) {
    //cout << "aktualizuj Pamiec" << endl;
    pamiecU.push_front(u);
    pamiecU.pop_back();
    //cout << "\npamiecU: " << pamiecU << endl;
    pamiecY.push_front(y);
    pamiecY.pop_back();
    //cout << "\npamiecY: " << pamiecY << endl;
}

//======= krokIdentyfikacji ====================================================
/**
 * Funkcja dokonujaca kolejnej iteracji metody RMNK.
 *      @param u - wartosc wejscia 
 *      @param y - wartosc wyjscia
 *      @return wektor ocen parametrow Theta
 */
Macierz Identyfikacja::krokIdentyfikacji(double u, double y) {
    numerProbki++;
    //cout << endl << "krok identyfikacji: " << numerProbki << endl;
    
    // ---------- aktualizacja wektorow pamieci i pomiarow ---------------------
    aktualizujPamiec(u, y);
    aktualizujFi();
    // ---------- obliczenie bledu ---------------------------------------------
    double epsylon = y - (wektorFi.transponuj()*wektorTheta).at(0, 0);
    //cout<<"epsylon = "<<epsylon<<endl;
    // ---------- obliczenie nowej macierzy P ----------------------------------
    macierzP = (macierzP - 
            ((macierzP*wektorFi*wektorFi.transponuj()*macierzP)
            /(alfa+wektorFi.transponuj()*macierzP*wektorFi)))/alfa;
    double slad = macierzP.obliczSlad();
    if(slad>2*(dB+1+dA+dC)*100){
        macierzP = 100*macierzP/slad;
        cout<<"przeskalowano macierz P"<<endl;
    }
    //cout<<"macierzP: "<<endl<<macierzP<<endl;
    // ---------- obliczenie wektora wzmocnien k -------------------------------
    wektorK = macierzP * wektorFi;
    //cout<<"wektorK: "<<endl<<wektorK<<endl;
    // ---------- uaktualnienie wektora ocen theta -----------------------------
    wektorTheta += wektorK * epsylon;
    // ---------- sprawdzenie poprawnosci ocen ---------------------------------
    for(int i = 0; i<wektorTheta.liczbaWierszy(); i++){
        if (!(wektorTheta(i, 0)==wektorTheta(i, 0))){
            throw Wyjatek("Blad w identyfikacji\n Ocena parametru nie jest liczba ");
        }
    }
    //cout<<"wektorTheta: "<<endl<<wektorTheta<<endl;
    // ---------- estymacja bledu ----------------------------------------------
    double e = y - (wektorFi.transponuj()*wektorTheta).at(0, 0);
    pamiecE.push_front(e);
    pamiecE.pop_back();
    return wektorTheta;
}

//======= stalySlad ====================================================
/**
 * Funkcja dokonujaca kolejnej iteracji metody RMNK.
 *      @param u - wartosc wejscia 
 *      @param y - wartosc wyjscia
 *      @return wektor ocen parametrow Theta
 */
Macierz Identyfikacja::stalySlad(double u, double y) {
    //numerProbki++;
    //cout << endl << "krok identyfikacji: " << numerProbki << endl;
    double c1 = 5;
    // ---------- aktualizacja wektorow pamieci i pomiarow ---------------------
    aktualizujPamiec(u, y);
    aktualizujFi();
    // ---------- obliczenie bledu ---------------------------------------------
    double epsylon = y - (wektorFi.transponuj()*wektorTheta).at(0, 0);
    //cout<<"epsylon = "<<epsylon<<endl;
    // ---------- obliczenie nowej macierzy P ----------------------------------
    macierzP = (macierzP - 
            ((macierzP*wektorFi*wektorFi.transponuj()*macierzP)
            /(1+wektorFi.transponuj()*macierzP*wektorFi)));
    macierzP = (c1/macierzP.obliczSlad())*macierzP;
    //cout<<"macierzP: "<<endl<<macierzP<<endl;
    // ---------- obliczenie wektora wzmocnien k -------------------------------
    wektorK = macierzP * wektorFi;
    //cout<<"wektorK: "<<endl<<wektorK<<endl;
    // ---------- uaktualnienie wektora ocen theta -----------------------------
    wektorTheta += wektorK * epsylon;
    for(int i = 0; i<wektorTheta.liczbaWierszy(); i++){
        if (!(wektorTheta(i, 0)==wektorTheta(i, 0))){
            throw Wyjatek("Blad w identyfikacji\n Ocena parametru nie jest liczba ");
        }
    }
    //cout<<"wektorTheta: "<<endl<<wektorTheta<<endl;
    // ---------- estymacja bledu ----------------------------------------------
    double e = y - (wektorFi.transponuj()*wektorTheta).at(0, 0);
    pamiecE.push_front(e);
    pamiecE.pop_back();
    return wektorTheta;
}

//======= pobierzTheta =========================================================
/**
 * Funkcja inspektor zwracajaca wartosc wektora parametrow.
 *      @return wektor ocen parametrow theta
 */
Macierz Identyfikacja::pobierzTheta() const{
    return wektorTheta;
}

//======= pobierzFi ============================================================
/**
 * Funkcja inspektor zwracajaca wartosc wektora pomiarow.
 *      @return wektor pomiarow fi
 */
Macierz Identyfikacja::pobierzFi() const{
    return wektorFi;
}

//======= pobierzA =============================================================
/**
 * Funkcja inspektor zwracajaca wspolczynniki wielomianu A.
 *      @return wektor wspolczynnikow wielomianu A
 */
vector<double> Identyfikacja::pobierzA() const{
    vector<double> wspA(dA+1, 1.0);
    for(int i=1; i<=dA; i++){
        wspA.at(i) = wektorTheta(dB+i, 0);
    }
    return wspA;
}

//======= pobierzB =============================================================
/**
 * Funkcja inspektor zwracajaca wspolczynniki wielomianu B.
 *      @return wektor wspolczynnikow wielomianu B
 */
vector<double> Identyfikacja::pobierzB() const{
    vector<double> wspB(dB+1, 1.0);
    for(int i=0; i<=dB; i++){
        wspB.at(i) = wektorTheta(i, 0);
    }
    return wspB;
}

//======= pobierzC =============================================================
/**
 * Funkcja inspektor zwracajaca wspolczynniki wielomianu C.
 *      @return wektor wspolczynnikow wielomianu C
 */
vector<double> Identyfikacja::pobierzC() const{
    vector<double> wspC(dC+1, 1.0);
    for(int i=1; i<=dC; i++){
        wspC.at(i) = wektorTheta(dB+dA+i, 0);
    }
    return wspC;
}



//------------------------------------------------------------------------------
//-------------- KONIEC [definicji klasy Identyfikacja] -------------------------------
//------------------------------------------------------------------------------
